/*
Generated c++ prg

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
*/

#include "../../dynlib.h"

void swing_up(Matrix &SimParam,Matrix &Xm,int i,int ui,double *U,ColumnVector &dX)
{
	//Declaracion de variables de tipo DOUBLE
	double signo, temp, temp2, Eo, w, E;

	//Declaracion de variables de tipo INT
	int fin, j;

	fin=(int)(SimParam(4,4)+SimParam(4,3));
	//Declaracion de matrices
	//Declaracion de vectores Columna
	ColumnVector X(fin);

	//Declaracion de vectores Fila

	for (j=1;j++;j<=fin){
	X(j)=Xm(j,i);}

	temp=1.0;
	Eo=10.0;
	if (X(4)*cos(X(2))>0.0)
	{
	   signo=1.0;
	}
	else
	{
	   signo=-1.0;
	}
	w=sqrt((SimParam(1,1)*SimParam(3,5)*SimParam(1,3))/(4*SimParam(1,4)));
	temp2=(X(4)/w)*(X(4)/w);
	E=SimParam(1,1)*SimParam(3,5)*SimParam(1,3)*(0.5*temp2+cos(X(2))-1);
	*U=((SimParam(2,2)/SimParam(2,1))*(SimParam(1,1)*temp*(E-Eo)*signo+(1/SimParam(2,2))*X(3)))*1/3;
		

}
